

#include "PoseEster.h"

PoseEster::PoseEster(string ParamFile) :
										 fs(ParamFile, FileStorage::READ)  // THIS IS OPENCV FileStorage problem
{

	fs["camera_matrix"] >> CamMat;
	fs["distortion_coefficients"] >> DistCoeff;

	MATmtxdisp(CamMat);
	MATmtxdisp(DistCoeff);
	getchar();

	CamParams.rho_u = CamMat.at<double>(0,0);
	CamParams.rho_v = CamMat.at<double>(1,1); 
	CamParams.uc = CamMat.at<double>(0,2);
	CamParams.vc = CamMat.at<double>(1,2);
}


PoseEster::PoseEster(FileStorage fs)
{

	fs["camera_matrix"] >> CamMat;
	fs["distortion_coefficients"] >> DistCoeff;

	MATmtxdisp(CamMat);
	MATmtxdisp(DistCoeff);
	getchar();

	CamParams.rho_u = CamMat.at<double>(0,0);
	CamParams.rho_v = CamMat.at<double>(1,1); 
	CamParams.uc = CamMat.at<double>(0,2);
	CamParams.vc = CamMat.at<double>(1,2);
}



PoseEster::~PoseEster()
{



}



int PoseEster::getPose(Mat p, Mat& P, double working_dist)
{
	int ret = 1;

	P = Mat::zeros(3,1, CV_64FC1);

    double Z0 = working_dist;
	
	double x = p.at<double>(0);
	double y = p.at<double>(1);

	double rho_u = CamParams.rho_u;
	double rho_v = CamParams.rho_v;
	double uc	 = CamParams.uc;
	double vc	 = CamParams.vc;

	double X, Y;

	if ((Z0 != -1) && (rho_u != 0) && (rho_v != 0))
	{
		X = (x*(Z0 + 1) - uc*(Z0 + 1))/rho_u;
		Y = (y*(Z0 + 1) - vc*(Z0 + 1))/rho_v;

	} else
	{
		printf("Error: getPose calculation!!");
		ret = 0;
	}

	P.at<double>(0) = X;
	P.at<double>(1) = Y;
	P.at<double>(2) = Z0;

	return ret;
}